#include "bsp.h"
#include "stm32f1xx_hal.h"                  // Device header
#include "OLED.h"
#include "MPU6050_Reg.h"
#include "MPU6050.h"
#include <stdio.h>
#include <stdbool.h>
#include "cmsis_os.h"
#include "SampleBufferInt16.h"

#define MPU6050_ADDRESS		0xD0
#define MPU6050_AXEL_NUM 6
#define SAMPLE_BUFFER_WINDOW_TIME 500	/* 采样窗口时间，决定OLED显示多少ms前的数据 */
#define SAMPLE_PERIOD 20

uint8_t MPU6050_ID;
int16_t MPU6050_RealData[MPU6050_AXEL_NUM] = {0};
int16_t MPU6050DisplayData[MPU6050_AXEL_NUM] = {0};

typedef enum
{
	CH_MPU6050_AX = 0,
	CH_MPU6050_AY,
	CH_MPU6050_AZ,
	CH_MPU6050_GX,
	CH_MPU6050_GY,
	CH_MPU6050_GZ
} MPU6050Channel;

SampleBufferInt16 MPU6050SampleBuffers[MPU6050_AXEL_NUM];
int16_t MPU6050SampleBuffersData[MPU6050_AXEL_NUM][SAMPLE_BUFFER_WINDOW_TIME / SAMPLE_PERIOD];

void MPU6050_WriteReg(uint8_t RegAddress, uint8_t Data)
{
	MyI2C_Start();
	MyI2C_SendByte(MPU6050_ADDRESS);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(RegAddress);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(Data);
	MyI2C_ReceiveAck();
	MyI2C_Stop();
}

/**
 * @brief 
 * 
 * @param RegAddress 
 * @return uint8_t 
 */
uint8_t MPU6050_ReadReg(uint8_t RegAddress)
{
	uint8_t Data;

	MyI2C_Start();
	MyI2C_SendByte(MPU6050_ADDRESS);
	MyI2C_ReceiveAck();
	MyI2C_SendByte(RegAddress);
	MyI2C_ReceiveAck();
	
	MyI2C_Start();
	MyI2C_SendByte(MPU6050_ADDRESS | 0x01);
	MyI2C_ReceiveAck();
	Data = MyI2C_ReceiveByte();
	MyI2C_SendAck(1);
	MyI2C_Stop();
	
	return Data;
}

void MPU6050_Init(void)
{
	if(!Get_I2C_InitState()){
		MyI2C_Init();
		printf("i2c uninited, init...\r\n");
	}

	MPU6050_WriteReg(MPU6050_PWR_MGMT_1, 0x01);		// 不复位，关闭睡眠模式，不循环，使能温度传感器，选择X轴陀螺仪的内部震荡电路作为系统时钟
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2, 0x00);		// 不需要设置循环模式的唤醒频率, 六个轴都不需要待机
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV, 0x09);		// 设置采样分频, 这里选择10分频
	MPU6050_WriteReg(MPU6050_CONFIG, 0x06);			// 不需要外部同步, 数字低通滤波设置为最高(最平滑)
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG, 0x18);	// 角速度计配置:不自测(高三位为自测使能, 手册有遗漏), 设计为最大量程
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG, 0x18);	// 加速度计配置:不自测,选择为最大量程,不使用高通滤波器
}

uint8_t MPU6050_GetID(void)
{
	return MPU6050_ReadReg(MPU6050_WHO_AM_I);
}

void MPU6050GetDisplayData(void){
	for(int i = 0; i < MPU6050_AXEL_NUM; i++)
	{
		MPU6050SampleBuffers[i].appendDataBack(&MPU6050SampleBuffers[i], MPU6050_RealData[i]);
		MPU6050DisplayData[i] = MPU6050SampleBuffers[i].getRootMeanSquare(MPU6050SampleBuffers[i].buffer, MPU6050SampleBuffers[i].bufferIndex);
	}
}


void MPU6050_GetSampleData()
{
	uint8_t DataH, DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_XOUT_L);
	MPU6050_RealData[CH_MPU6050_AX] = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_YOUT_L);
	MPU6050_RealData[CH_MPU6050_AY] = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_ACCEL_ZOUT_L);
	MPU6050_RealData[CH_MPU6050_AZ] = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_XOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_XOUT_L);
	MPU6050_RealData[CH_MPU6050_GX]  = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_YOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_YOUT_L);
	MPU6050_RealData[CH_MPU6050_GY]  = (DataH << 8) | DataL;
	
	DataH = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_H);
	DataL = MPU6050_ReadReg(MPU6050_GYRO_ZOUT_L);
	MPU6050_RealData[CH_MPU6050_GZ]  = (DataH << 8) | DataL;

}

void MPU6050SampleTaskHandler(const void *pv)
{
	for(int i = 0; i < MPU6050_AXEL_NUM; i++){
		SampleBufferInt16Init(&MPU6050SampleBuffers[i], MPU6050SampleBuffersData[i], SAMPLE_BUFFER_WINDOW_TIME /SAMPLE_PERIOD);
	}

	while(1){
		MPU6050_GetSampleData();
		MPU6050GetDisplayData();
		vTaskDelay(SAMPLE_PERIOD);
	}
}

void MPU6050DisplayTaskHandler(const void *pv)
{
	OLED_Init();
	MPU6050_Init();
	
	OLED_ShowString(1, 1, "ID:");
	MPU6050_ID = MPU6050_GetID();
	OLED_ShowHexNum(1, 4, MPU6050_ID, 2);
	printf("MPU6050 ID = %d\r\n", MPU6050_ID);
	while (1)
	{
		OLED_ShowSignedNum(2, 1, MPU6050DisplayData[CH_MPU6050_AX], 5);
		OLED_ShowSignedNum(3, 1, MPU6050DisplayData[CH_MPU6050_AY], 5);
		OLED_ShowSignedNum(4, 1, MPU6050DisplayData[CH_MPU6050_AZ], 5);
		OLED_ShowSignedNum(2, 8, MPU6050DisplayData[CH_MPU6050_GX], 5);
		OLED_ShowSignedNum(3, 8, MPU6050DisplayData[CH_MPU6050_GY], 5);
		OLED_ShowSignedNum(4, 8, MPU6050DisplayData[CH_MPU6050_GZ], 5);

		// OLED_ShowSignedNum(2, 1, MPU6050_AX_Real, 5);
		// OLED_ShowSignedNum(3, 1, MPU6050_AY_Real, 5);
		// OLED_ShowSignedNum(4, 1, MPU6050_AZ_Real, 5);
		// OLED_ShowSignedNum(2, 8, MPU6050_GX_Real, 5);
		// OLED_ShowSignedNum(3, 8, MPU6050_GY_Real, 5);
		// OLED_ShowSignedNum(4, 8, MPU6050_GZ_Real, 5);
		vTaskDelay(20);
	}
}

void MPU6050_Test(void)
{
	osThreadDef(MPU6050DisplayTask, MPU6050DisplayTaskHandler, osPriorityNormal, 0, 256);
	osThreadCreate(osThread(MPU6050DisplayTask), NULL);
	osThreadDef(MPU6050SampleTask, MPU6050SampleTaskHandler, osPriorityNormal, 0, 256);
	osThreadCreate(osThread(MPU6050SampleTask), NULL);
}